#include "feg_robot.h"
/**************************************
功能: 主函数，ROS初始化，通过feg_robot类创建Robot_control对象并自动调用构造函数初始化
***************************************/
int main(int argc, char** argv)
{
  ros::init(argc, argv, "feg_robot"); //ROS初始化 并设置节点名称 
  feg_robot Robot_Control; //实例化一个对象
  Robot_Control.Control(); //循环执行数据采集和发布话题等操作
  ROS_INFO("feg_robot node has turned on "); //提示已创建feg_robot节点 
  return 0;  
} 

/**************************************
功能: 数据转化函数 输入串口消息  一个高位一个低位  返回合并后的数值
***************************************/
short feg_robot::Data_Trans(uint8_t Data_High,uint8_t Data_Low)
{
  short transition_16;
  transition_16 = 0;
  transition_16 |=  Data_High<<8;  //获取数据的高8位
  transition_16 |=  Data_Low;      //获取数据的低8位
  return transition_16;
}

/**************************************
功能: 速度话题订阅回调函数Callback，根据订阅的指令通过串口发指令控制下位机
***************************************/
void feg_robot::Cmd_Vel_Callback(const geometry_msgs::Twist &twist_aux)
{
  short  transition;  //中间变量

  Send_Data.tx[0]=FRAME_HEADER1; //帧头1 0XAA
  Send_Data.tx[1]=FRAME_HEADER2; //帧头2 0X55
  Send_Data.tx[2] = 0X01; //驱动器地址 默认0x01
  Send_Data.tx[3] = 0x03; //左右轮单独控制  线速度角速度模式：0x02 左右轮单独控制：0x03
  Send_Data.tx[4] = 0x04; //数据长度 
  Send_Data.left_speed=LEFT_SPEED_K*(2*twist_aux.linear.x-twist_aux.angular.z*WHEEL_DIAMETER)/2.0;
  Send_Data.right_speed=RIGHT_SPEED_K*(2*twist_aux.linear.x+twist_aux.angular.z*WHEEL_DIAMETER)/2.0;
  //左电机线速度
  transition=0;
  transition = Send_Data.left_speed*1000; //将浮点数放大一千倍，m/s 转化成 mm/s
  Send_Data.tx[6] = transition;     //取数据的低8位
  Send_Data.tx[5] = transition>>8;  //取数据的高8位
  //右电机线速度
  transition=0;
  transition = Send_Data.right_speed*1000;
  Send_Data.tx[8] = transition; //取数据的低8位
  Send_Data.tx[7] = transition>>8;  //取数据的高8位
  
  //电机运动模式  
  //1：里程计清零;
  // 0：/
  // Bit2&Bit1:
  // 00：进入自由滑行状态（电机停止工作）
  // 01：进入运行状态
  // 11：进入刹车状态
  // 10：锁轴
  Send_Data.tx[9] = 0x02;  //进入运行状态
  Send_Data.tx[10] = 0x00;  //保留位
  Send_Data.tx[11]=Check_Sum(11,SEND_DATA_CHECK); //CRC校验位，规则参见Check_Sum函数
  Send_Data.tx[12]=FRAME_TAIL; //帧尾0XCC
  try
  {
    Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //通过串口向下位机发送数据 
  }
  catch (serial::IOException& e)   
  {
    ROS_ERROR_STREAM("Unable to send data through serial port"); //如果发送数据失败，打印错误信息
  }
}

/**************************************
功能: 发布里程计话题，包含位置、姿态、三轴速度、绕三轴角速度、TF父子坐标、协方差矩阵
***************************************/
void feg_robot::Publish_Odom()
{
    geometry_msgs::TransformStamped odom_trans; //实例化TF消息数据 
    geometry_msgs::Quaternion odom_quat;  //Z轴转角四元数
    nav_msgs::Odometry odom;   //实例化里程计话题数据
    odom.header.stamp = ros::Time::now(); 
    odom.header.frame_id = odom_frame_id;      //里程计TF父坐标
    odom.pose.pose.position.x = Robot_Pos.X;   //x位置
    odom.pose.pose.position.y = Robot_Pos.Y;
    odom.pose.pose.position.z = 0.0;
    odom_quat = tf::createQuaternionMsgFromYaw(Robot_Pos.Z); //把Z轴转角转换为四元数进行表达
    odom.pose.pose.orientation = odom_quat;    //姿态，通过Z轴转角转换的四元数
    odom.child_frame_id = robot_frame_id;      //里程计TF子坐标
    odom.twist.twist.linear.x =  Robot_Vel.X;  //X方向速度
    odom.twist.twist.linear.y =  Robot_Vel.Y;  //Y方向速度
    odom.twist.twist.angular.z = Robot_Vel.Z;  //绕Z轴角速度 
    //这个矩阵有两种，分别在机器人静止和运动的时候使用。扩展卡尔曼滤波官方提供的2个矩阵，用于robot_pose_ekf功能包
    if(Robot_Vel.X== 0&&Robot_Vel.Y== 0&&Robot_Vel.Z== 0)
      //如果velocity是零，说明编码器的误差会比较小，认为编码器数据更可靠
      memcpy(&odom.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2)),
      memcpy(&odom.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2));
    else
      //如果小车velocity非零，考虑到运动中编码器可能带来的滑动误差，认为imu的数据更可靠
      memcpy(&odom.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance)),
      memcpy(&odom.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance));       
    odom_publisher.publish(odom);  //发布里程计话题
}

/**************************************
功能: 发布TF变换
***************************************/
void feg_robot::Publish_TF()
{
    geometry_msgs::TransformStamped odom_trans; //实例化TF消息数据 
    geometry_msgs::Quaternion odom_quat;  //把Z轴转角转换为四元数进行表达
    odom_trans.header.stamp = ros::Time::now();
    odom_trans.header.frame_id = odom_frame_id;
    odom_trans.child_frame_id  = robot_frame_id;
    odom_quat = tf::createQuaternionMsgFromYaw(Robot_Pos.Z); //把Z轴转角转换为四元数进行表达
    odom_trans.transform.translation.x = Robot_Pos.X;
    odom_trans.transform.translation.y = Robot_Pos.Y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;
    odom_broadcaster.sendTransform(odom_trans);
}
/**************************************
功能: 发布电压相关信息
***************************************/
void feg_robot::Publish_Voltage()
{
    std_msgs::Float32 voltage_msgs;  //定义电源电压发布话题的数据类型
    static float Count_Voltage_Pub=0;
    if(Count_Voltage_Pub++>10)
      {
        Count_Voltage_Pub=0;  
        voltage_msgs.data = Power_voltage; //电源供电的电压获取
        voltage_publisher.publish(voltage_msgs);  //发布电源电压话题单位：V、伏特
      }
}

/**************************************
功能: 串口通讯校验函数，数据包n有个字节，第n-1个字节为校验位，第n个字节位帧尾。第1个字节到第n-2个字节数据相加与第n-1个字节对比，即为CRC校验
输入参数： Count_Number：数据包前几个字节加入校验   mode：对发送数据还是接收数据进行校验
***************************************/
unsigned char feg_robot::Check_Sum(unsigned char Count_Number,unsigned char mode)
{
  unsigned char check_sum=0,k;
  
  if(mode==0)  //接收数据模式
  {
   for(k=0;k<Count_Number;k++)
    {
     check_sum+=Receive_Data.rx[k];  //CRC 校验,相加
     }
  }
  if(mode==1) //发送数据模式
  {
   for(k=0;k<Count_Number;k++)
    {
     check_sum+=Send_Data.tx[k]; //CRC 校验,相加
     }
  }
  return check_sum;  //返回校验相加结果
}

/**************************************
功能: 通过串口读取并校验下位机发送过来的数据，然后数据转换为国际单位
***************************************/
bool feg_robot::Get_Sensor_Data()
{ 
  short transition_16=0, j=0, Header_Pos=0, Tail_Pos=0; //中间变量
  uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE]={0}; //临时变量，保存下位机数据
  //std_msgs::string input;
  //if (Stm32_Serial.available())
  //{
  Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr)); //通过串口读取下位机发送过来的数据
  //}
  //直接查看接收到的原始数据，调试使用
  // ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x",
  // Receive_Data_Pr[0],Receive_Data_Pr[1],Receive_Data_Pr[2],Receive_Data_Pr[3],Receive_Data_Pr[4],Receive_Data_Pr[5],Receive_Data_Pr[6],Receive_Data_Pr[7],
  // Receive_Data_Pr[8],Receive_Data_Pr[9],Receive_Data_Pr[10],Receive_Data_Pr[11],Receive_Data_Pr[12],Receive_Data_Pr[13],Receive_Data_Pr[14],Receive_Data_Pr[15],
  // Receive_Data_Pr[16],Receive_Data_Pr[17],Receive_Data_Pr[18],Receive_Data_Pr[19],Receive_Data_Pr[20],Receive_Data_Pr[21],Receive_Data_Pr[22],Receive_Data_Pr[23],
  // Receive_Data_Pr[24],Receive_Data_Pr[25],Receive_Data_Pr[26],Receive_Data_Pr[27],Receive_Data_Pr[28],Receive_Data_Pr[29],Receive_Data_Pr[30],Receive_Data_Pr[31],
  // Receive_Data_Pr[32],Receive_Data_Pr[33],Receive_Data_Pr[34],Receive_Data_Pr[35]);

  //记录帧头帧尾位置
  for(j=0;j<36;j++)
  {
    if(Receive_Data_Pr[j]==FRAME_HEADER1)
    Header_Pos=j;
    else if(Receive_Data_Pr[j]==FRAME_TAIL)
    Tail_Pos=j;    
  }

  if(Tail_Pos==(Header_Pos+35))
  {
    //如果帧尾在数据包最后一位，直接复制数据包到Receive_Data.rx
    // ROS_INFO("1----");
    memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr));
  }
  else if(Header_Pos==(1+Tail_Pos))
  {
    //如果帧头在帧尾后面，纠正数据位置后复制数据包到Receive_Data.rx
    // If the header is behind the end of the frame, copy the packet to receive_data.rx after correcting the data location
    // ROS_INFO("2----");
    for(j=0;j<36;j++)
    Receive_Data.rx[j]=Receive_Data_Pr[(j+Header_Pos)%36];
  }
  else 
  {
    //其它情况则认为数据包有错误
    // ROS_INFO("3----");
    return false;
  }    
  //查看Receive_Data.rx，调试使用
  // ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x",
  // Receive_Data.rx[0],Receive_Data.rx[1],Receive_Data.rx[2],Receive_Data.rx[3],Receive_Data.rx[4],Receive_Data.rx[5],Receive_Data.rx[6],Receive_Data.rx[7],
  // Receive_Data.rx[8],Receive_Data.rx[9],Receive_Data.rx[10],Receive_Data.rx[11],Receive_Data.rx[12],Receive_Data.rx[13],Receive_Data.rx[14],Receive_Data.rx[15],
  // Receive_Data.rx[16],Receive_Data.rx[17],Receive_Data.rx[18],Receive_Data.rx[19],Receive_Data.rx[20],Receive_Data.rx[21],Receive_Data.rx[22],Receive_Data.rx[23],
  // Receive_Data.rx[24],Receive_Data.rx[25],Receive_Data.rx[26],Receive_Data.rx[27],Receive_Data.rx[28],Receive_Data.rx[29],Receive_Data.rx[30],Receive_Data.rx[31],
  // Receive_Data.rx[32],Receive_Data.rx[33],Receive_Data.rx[34],Receive_Data.rx[35]); 
  
  Receive_Data.Frame_Header= Receive_Data.rx[0]; //数据的第一位是帧头1:0XAA
  Receive_Data.Frame_Tail= Receive_Data.rx[35];  //数据的最后一位是帧尾:0XCC

  if (Receive_Data.Frame_Header == FRAME_HEADER1 )  //判断帧头
  {
    if (Receive_Data.Frame_Tail == FRAME_TAIL)  //判断帧尾
    { 
      if (Receive_Data.rx[34] == Check_Sum(34,READ_DATA_CHECK)) //CRC校验通过或者两组数据包交错
      {
        Robot_Vel.left_enc = Data_Trans(Receive_Data.rx[7],Receive_Data.rx[8]); ////获取左轮encoder
        Robot_Vel.right_enc = Data_Trans(Receive_Data.rx[5],Receive_Data.rx[6]); //获取右轮encoder
        Power_voltage=Data_Trans(Receive_Data.rx[25],Receive_Data.rx[26])/10.0; 
        //data_return   =  (transition_16 / 1000)+(transition_16 % 1000)*0.001; //缩小1000倍 成为小数
        return true;
      }
    }
  } 
  return false;
}

/**************************************
功能: 循环获取下位机数据与发布话题
***************************************/
void feg_robot::Control()
{
  while(false == Get_Sensor_Data()){}
  _Last_Time = ros::Time::now();
  Robot_Vel.last_left_enc=Robot_Vel.left_enc;
  Robot_Vel.last_right_enc=Robot_Vel.right_enc;
  ROS_INFO("left_enc_start:%d",Robot_Vel.last_left_enc);
  ROS_INFO("right_enc_start:%d",Robot_Vel.last_right_enc);
  while(ros::ok())
  {
    _Now = ros::Time::now();
    Sampling_Time = (_Now - _Last_Time).toSec(); //获取时间间隔，用于积分速度获得位移(里程)                                            
    if (true == Get_Sensor_Data()) //通过串口读取并校验下位机发送过来的数据，然后数据转换为国际单位                               
    {
      Robot_Vel.left_change=Robot_Vel.left_enc-Robot_Vel.last_left_enc;
      Robot_Vel.right_change=Robot_Vel.right_enc-Robot_Vel.last_right_enc;
      if(Robot_Vel.left_change>30000) Robot_Vel.left_change-=ENCODER_MAX;
      if(Robot_Vel.left_change<-30000) Robot_Vel.left_change+=ENCODER_MAX;
      if(Robot_Vel.right_change>30000) Robot_Vel.right_change-=ENCODER_MAX;
      if(Robot_Vel.right_change<-30000) Robot_Vel.right_change+=ENCODER_MAX;
      Robot_Vel.last_left_enc=Robot_Vel.left_enc;
      Robot_Vel.last_right_enc=Robot_Vel.right_enc;
      Robot_Vel.left_d=(Robot_Vel.left_change)*ticks_per_meter;
      Robot_Vel.right_d=(Robot_Vel.right_change)*ticks_per_meter;
      Robot_Vel.d_xy_avg=(Robot_Vel.left_d+Robot_Vel.right_d)/2;
      Robot_Vel.d_th=(Robot_Vel.right_d-Robot_Vel.left_d)/WHEEL_TRACK;
      Robot_Vel.X=Robot_Vel.d_xy_avg/Sampling_Time;
      Robot_Vel.Z=Robot_Vel.d_th/Sampling_Time;

      if (Robot_Vel.d_xy_avg!=0)
      {
        Robot_Pos.dx=cos(Robot_Vel.d_th)*Robot_Vel.d_xy_avg;
        Robot_Pos.dy=sin(Robot_Vel.d_th)*Robot_Vel.d_xy_avg;
        Robot_Pos.X+=cos(Robot_Pos.Z)*Robot_Pos.dx-sin(Robot_Pos.Z)*Robot_Pos.dy;
        Robot_Pos.Y+=sin(Robot_Pos.Z)*Robot_Pos.dx+cos(Robot_Pos.Z)*Robot_Pos.dy;
      }
      if(Robot_Vel.d_th!=0) Robot_Pos.Z+=Robot_Vel.d_th;
    }
    Publish_Odom();        //发布里程计话题
    Publish_TF();  
    Publish_Voltage();     //发布电源电压话题
    _Last_Time = _Now; //记录时间，用于计算时间间隔
    ros::spinOnce();   //循环等待回调函数
  }
}
/**************************************
功能: 构造函数, 只执行一次，用于初始化
***************************************/
feg_robot::feg_robot():Sampling_Time(0),Power_voltage(0)
{
  //清空数据
  memset(&Robot_Pos, 0, sizeof(Robot_Pos));
  memset(&Robot_Vel, 0, sizeof(Robot_Vel));
  memset(&Receive_Data, 0, sizeof(Receive_Data)); 
  memset(&Send_Data, 0, sizeof(Send_Data));

  ros::NodeHandle private_nh("~"); //创建节点句柄
  //private_nh.param()入口参数分别对应：参数服务器上的名称  参数变量名  初始值
  private_nh.param<std::string>("usart_port_name",  usart_port_name,  "/dev/feg");  //固定串口号
  private_nh.param<int>        ("serial_baud_rate", serial_baud_rate, 115200);      //和下位机通信波特率115200
  private_nh.param<std::string>("cmd_vel",          cmd_vel,          "cmd_vel");   //速度控制话题名
  private_nh.param<std::string>("odom_frame_id",    odom_frame_id,    "odom");      //里程计话题对应父TF坐标
  private_nh.param<std::string>("robot_frame_id",   robot_frame_id,   "base_link"); //里程计话题对应子TF坐标

  voltage_publisher = n.advertise<std_msgs::Float32>("PowerVoltage", 10); //创建电池电压话题发布者
  odom_publisher    = n.advertise<nav_msgs::Odometry>("odom", 50); //创建里程计话题发布者

  //速度控制命令订阅回调函数设置
  Cmd_Vel_Sub  = n.subscribe(cmd_vel,100, &feg_robot::Cmd_Vel_Callback, this); 
  ROS_INFO_STREAM("Data ready");  //提示信息
  
  try
  { 
    //尝试初始化与开启串口
    ROS_INFO_STREAM("start connect FEG robot..."); //串口开启成功提示
    Stm32_Serial.setPort(usart_port_name); //选择要开启的串口号
    Stm32_Serial.setBaudrate(serial_baud_rate);  //设置波特率
    serial::Timeout _time = serial::Timeout::simpleTimeout(2000);//超时等待
    Stm32_Serial.setTimeout(_time);
    Stm32_Serial.open(); //开启串口
  }
  catch (serial::IOException& e)
  {
    ROS_ERROR_STREAM("feg_robot can not open serial port,Please check the serial port cable! "); //如果开启串口失败，打印错误信息
  }
  if(Stm32_Serial.isOpen())
  {
    ROS_INFO_STREAM("feg_robot serial connect succeed!"); //串口开启成功提示
  }
}

/**************************************
功能: 析构函数，只执行一次，当对象结束其生命周期时系统会调用这个函数
***************************************/
feg_robot::~feg_robot()
{
  //对象feg_robot结束前向下位机发送停止运动命令
  short  transition; //中间变量
  Send_Data.tx[0]=FRAME_HEADER1; //帧头1 0XAA
  Send_Data.tx[1]=FRAME_HEADER2; //帧头2 0X55
  Send_Data.tx[2] = 0X01; //驱动器地址 默认0x01
  Send_Data.tx[3] = 0x02; //控制模式 线速度角速度模式：0x02 左右轮单独控制：0x03
  Send_Data.tx[4] = 0x04; //数据长度 

  //机器人x轴的目标线速度0
  Send_Data.tx[6] = 0;     //取数据的低8位
  Send_Data.tx[5] = 0;     //取数据的高8位

  //机器人z轴的目标角速度0
  Send_Data.tx[8] = 0; //取数据的低8位
  Send_Data.tx[7] = 0;  //取数据的高8位
  
  //电机运动模式  
  // 00：进入自由滑行状态（电机停止工作）
  // 02：进入运行状态
  // 11：进入刹车状态
  // 10：锁轴
  Send_Data.tx[9] = 0x00;  //进入自由滑行状态
  Send_Data.tx[10] = 0x00;  //保留位
  Send_Data.tx[11]=Check_Sum(11,SEND_DATA_CHECK); //CRC校验位，规则参见Check_Sum函数
  Send_Data.tx[12]=FRAME_TAIL; //帧尾0XCC

  try
  {
    Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //向串口最后一次发数据  
  }
  catch (serial::IOException& e)   
  {
    ROS_ERROR_STREAM("Unable to send data through serial port"); //如果发送数据失败,打印错误信息
  }
  Stm32_Serial.close(); //关闭串口  
  ROS_INFO_STREAM("Shutting down"); //提示信息
}